#ifndef _ANYANGLEINTERFACE_H
#define _ANYANGLEINTERFACE_H

#include <stdio.h>
#include <stdint.h>
#include <numeric>
#include "sys/time.h"
#include "ThetaStar.h"
#include <global_planner/planner_core.h>
#include <global_planner/expander.h>
#include <vector>
#include <algorithm>

namespace global_planner{
    class AnyAngleExpansion : public Expander {
    public:
        AnyAngleExpansion(PotentialCalculator* p_calc, int nx, int ny);
        bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles,
                                float* potential);
        bool path_find(unsigned char* costs,double resolution, unsigned int start_x, unsigned int start_y, unsigned int  end_x, unsigned int end_y, 
                       std::vector<std::vector<unsigned int> >*path);
        bool isFilledPath()
        {
            return fill_path;
        }
        void setFilledPath()
        {
            fill_path = true;
        }
        void setAnyanglePath()
        {
            fill_path = false;
        }
        void bresenham(int x1,int y1,int x2,int y2,std::vector<int> &grid_free_x,std::vector<int> &grid_free_y);
    private:
        bool get_path(unsigned char* costs, double resolution,int startPoint[2],int stopPoint[2],
                        int Smap_start[2],std::vector<std::vector<unsigned int> > *path);
        
        std::vector<bool> mapData;

        int width_map{0};
        int height_map{0};

        double const_shift_s[2];
        double const_shift_e[2];
        int N_width{0};
        int N_height{0};  

        bool fill_path{true};      
    };
}
#endif